/**
 * @file gyro.c
 * @author Sevenfite (Sevenfite@163.com)
 * @brief 陀螺仪抽象层
 * @version 0.1
 * @date 2024-05-20
 * 
 * @copyright Copyright (c) 2024
 * 
 */
#include "gyro.h"
#include "WT61.h"
#include "C_code_Log.h"
#include "usart_port.h"
#include "usart.h"
static float gl_Pitch,gl_Roll,gl_Yaw;//俯仰、横滚、偏航角 (放置位置不确定)
USART_Data gyroData;

static void gyro_dataBufferInit();
void gyro_callback(void* this);

void gyro_init(void) {
  // WT61_SetBaudrate(9600);  有bug
  gyro_dataBufferInit();
  WT61_Init();
  LOG_INFO("gyro init\n");
}

static uint8_t dataBuffer[33];	// 待完善
//static USART_Data gyroData;
static void gyro_dataBufferInit()
{
    USART_DataTypeInit(&gyroData,GYRO_USART_HANDLE,dataBuffer,sizeof(dataBuffer),DMA_MODE,gyro_callback);
    LOG_INFO("gyro dataBufferInit\n");
}

void gyro_callback(void* this)
{
	if(((USART_Data*)this) == &gyroData)
	{
		for (uint8_t i = 0; i < 33; i++)
		{
			CopeSerialData(gyroData.usart_rx_buf[i]);	//应该存在更好的查找数据头的方法
		}
		gl_Pitch = stcAngle.Angle[0];	// 绕X轴
		gl_Roll = stcAngle.Angle[1];	// 绕Y轴
		gl_Yaw = stcAngle.Angle[2];	// 绕Z轴
	}
}
// int8_t gyro_getData(){return 1;}//先过一下编译
/// @brief 陀螺仪获取数据
/// @note 检测陀螺仪串口，并处理数据，存储到本地变量中，如果要查看，可以调用对应的get函数
/// @example while(gyro_getData() != 1);pitch=gyro_getPitch;
/// @return 1 获取成功, 0 还未接收完成，-1 获取失败, 数据长度不对
int8_t gyro_getData()
{
   if(USART_DataIsReceived(&gyroData))
   {
       uint16_t len = USART_DataGetReceivedLen(&gyroData);
       USART_DataResetReceivedFlag(&gyroData);
       if(len == 33)
       {
          return 1;
       }
       return -1;
   }
   return 0;
}

/**
 * @brief 获取俯仰角
 * 
 * @return float 
 * @note 规定上坡时为负值，下坡时为正值
 */
float gyro_getPitch()
{
	
    return gl_Pitch;
}
/**
 * @brief 获取横滚角
 * 
 * @return float 
 */
float gyro_getRoll()
{

    return gl_Roll;
}
/**
 * @brief 获取偏航角
 * 
 * @return float 
 */
float gyro_getYaw()
{

    return gl_Yaw;
}
int gyro_getYawInt()
{
	return (int)gl_Yaw;
}


/*----------------------Test----------------------------*/
#include "display.h"
#include "ABS_key.h"
#include "myconfig.h"
My_StatusTypeDef Gyro_Test(int key_Num) {
  char str[20];
  sprintf(str, "Pitch:%.2f  ", gyro_getPitch());
  displayStr(str, Display_firstCol, Display_firstRow, RED);
  sprintf(str, "Roll:%.2f  ", gyro_getRoll());
  displayStr(str, Display_firstCol, Display_secondRow, RED);
  sprintf(str, "Yaw:%.2f  ", gyro_getYaw());
  displayStr(str, Display_firstCol, Display_thirdRow, RED);
  if (key_Num == Key_ESC) {
    return MY_OK;
  }
  return MY_BUSY;
}